/*
 * SsrrPlanning.h
 *
 *  Created on: Oct 22, 2013
 *      Author: incubed
 */

#ifndef SSRRPLANNING_H_
#define SSRRPLANNING_H_

//#include "omplapp/apps/AppBase.h"
#include <ompl/control/SimpleSetup.h>
#include <ompl/base/ScopedState.h>
#include <ompl/base/spaces/SE2StateSpace.h>
#include <ompl/control/ODESolver.h>
#include <ompl/control/spaces/RealVectorControlSpace.h>
#include <ompl/control/SpaceInformation.h>

/*namespace ompl {

	namespace control {
		typedef boost::shared_ptr<ODESolver> ODESolverPtr;
	}
}*/
//class ompl::control::ODESolverPtr;

namespace ssrr {

class SsrrPlanning : public ompl::control::SimpleSetup /*: public ompl::app::AppBase<ompl::app::CONTROL> */{

public:
	SsrrPlanning();
	SsrrPlanning(const ompl::control::ControlSpacePtr &controlSpace);
	virtual ~SsrrPlanning() {}

	bool isSelfCollisionEnabled(void) const
	{
		return false;
	}
	virtual unsigned int getRobotCount(void) const
	{
		return 1;
	}
	virtual ompl::base::ScopedState<> getDefaultStartState(void) const;
	virtual ompl::base::ScopedState<> getFullStateFromGeometricComponent(const ompl::base::ScopedState<> &state) const
	            		{
		return state;
	            		}
	virtual const ompl::base::StateSpacePtr& getGeometricComponentStateSpace(void) const
	{
		return si_->getStateSpace();
	}
	double getRadius()
	{
		return radius_;
	}
	void setRadius(double radius)
	{
		radius_ = radius;
	}
	virtual void setDefaultControlBounds();

	const std::string& getName()
	{
		return name_;
	}

protected:

	virtual const ompl::base::State* getGeometricComponentStateInternal(const ompl::base::State* state, unsigned int /*index*/) const
	{
		return state;
	}

	virtual void ode(const ompl::control::ODESolver::StateType& q, const ompl::control::Control *ctrl, ompl::control::ODESolver::StateType& qdot);

	virtual void postPropagate(const ompl::control::Control* control, ompl::base::State* state);

	static ompl::control::ControlSpacePtr constructControlSpace(void)
	{
		return ompl::control::ControlSpacePtr(new ompl::control::RealVectorControlSpace(constructStateSpace(), 2));
	}
	static ompl::base::StateSpacePtr constructStateSpace(void)
	{
		return ompl::base::StateSpacePtr(new ompl::base::SE2StateSpace());
	}


	double timeStep_;
	double radius_;
	boost::shared_ptr<ompl::control::ODESolver> odeSolver_;
	std::string name_;


};


}


#endif /* SSRRPLANNING_H_ */
